/*
 * L298N.h
 * 
 */
#ifndef L298N_H
#define L298N_H

#include <stdio.h>
#include <wiringPi.h>
#include <softPwm.h>

class Motor_4{
private:
	int speed_val = 200; //0~1024
	int speed_turn = 100;

	char Motor1_PWM ;
	char Motor1_IN0 ;
	char Motor1_IN1 ; 
	
	char Motor2_PWM ;
	char Motor2_IN0 ;
	char Motor2_IN1 ;

	char Motor3_PWM ;
	char Motor3_IN0 ;
	char Motor3_IN1 ;

	char Motor4_PWM ;
	char Motor4_IN0 ;
	char Motor4_IN1 ;

public:
	Motor_4(){}
	
	void setM1(char pwm, char in0, char in1)
	{
		Motor1_PWM = pwm;
		Motor1_IN0 = in0;
		Motor1_IN1 = in1;
	}
	
	void setM2(char pwm, char in0, char in1)
	{
		Motor2_PWM = pwm;
		Motor2_IN0 = in0;
		Motor2_IN1 = in1;
	}
	
	void setM3(char pwm, char in0, char in1)
	{
		Motor3_PWM = pwm;
		Motor3_IN0 = in0;
		Motor3_IN1 = in1;
	}
	
	void setM4(char pwm, char in0, char in1)
	{
		Motor4_PWM = pwm;
		Motor4_IN0 = in0;
		Motor4_IN1 = in1;
	}

	void init(void)
	{
		
		pinMode(Motor1_IN0,OUTPUT);
		pinMode(Motor1_IN1,OUTPUT);
		pinMode(Motor2_IN0,OUTPUT);
		pinMode(Motor2_IN1,OUTPUT);
		pinMode(Motor3_IN0,OUTPUT);
		pinMode(Motor3_IN1,OUTPUT);
		pinMode(Motor4_IN0,OUTPUT);
		pinMode(Motor4_IN1,OUTPUT);
		
		softPwmCreate(Motor1_PWM, speed_val, 1000);
		softPwmCreate(Motor2_PWM, speed_val, 1000);
		softPwmCreate(Motor3_PWM, speed_val, 1000);
		softPwmCreate(Motor4_PWM, speed_val, 1000);
	}

	void run_forward(void)
	{
		softPwmWrite (Motor1_PWM, speed_val);
		softPwmWrite (Motor2_PWM, speed_val);
		softPwmWrite (Motor3_PWM, speed_val);
		softPwmWrite (Motor4_PWM, speed_val);
		
		digitalWrite(Motor1_IN0,1);
		digitalWrite(Motor1_IN1,0);
		
		digitalWrite(Motor2_IN0,1);
		digitalWrite(Motor2_IN1,0);
		
		digitalWrite(Motor3_IN0,1);
		digitalWrite(Motor3_IN1,0);
		
		digitalWrite(Motor4_IN0,1);
		digitalWrite(Motor4_IN1,0);
		
		printf("正转...\n");
	}

	void run_backward(void)
	{
		softPwmWrite (Motor1_PWM, speed_val);
		softPwmWrite (Motor2_PWM, speed_val);
		softPwmWrite (Motor3_PWM, speed_val);
		softPwmWrite (Motor4_PWM, speed_val);
		
		digitalWrite(Motor1_IN0,0);
		digitalWrite(Motor1_IN1,1);
		
		digitalWrite(Motor2_IN0,0);
		digitalWrite(Motor2_IN1,1);
		
		digitalWrite(Motor3_IN0,0);
		digitalWrite(Motor3_IN1,1);
		
		digitalWrite(Motor4_IN0,0);
		digitalWrite(Motor4_IN1,1);
		
		printf("反转...\n");
	}

	void stop(void)
	{
		softPwmWrite (Motor1_PWM, 0);
		softPwmWrite (Motor2_PWM, 0);
		softPwmWrite (Motor3_PWM, 0);
		softPwmWrite (Motor4_PWM, 0);
		
		digitalWrite(Motor1_IN0,0);
		digitalWrite(Motor1_IN1,0);
		
		digitalWrite(Motor2_IN0,0);
		digitalWrite(Motor2_IN1,0);
		
		digitalWrite(Motor3_IN0,0);
		digitalWrite(Motor3_IN1,0);
		
		digitalWrite(Motor4_IN0,0);
		digitalWrite(Motor4_IN1,0);
		
		printf("停止...\n");
	}
	
	void turn_left(void)
	{
		softPwmWrite (Motor1_PWM, 0);
		softPwmWrite (Motor2_PWM, speed_turn);
		softPwmWrite (Motor3_PWM, speed_turn);
		softPwmWrite (Motor4_PWM, 0);
		
		printf("左转...\n");
	}

	void turn_right(void)
	{
		softPwmWrite (Motor1_PWM, speed_turn);
		softPwmWrite (Motor2_PWM, 0);
		softPwmWrite (Motor3_PWM, 0);
		softPwmWrite (Motor4_PWM, speed_turn);
		
		printf("右转...\n");
	}

	void speedUp(void)
	{
		speed_val += 100;
		if(speed_val>=1024) speed_val=1000;
		softPwmWrite (Motor1_PWM, speed_val);
		softPwmWrite (Motor2_PWM, speed_val);
		softPwmWrite (Motor3_PWM, speed_val);
		softPwmWrite (Motor4_PWM, speed_val);
		printf("加速: %d %%\n",speed_val);
	}

	void speedDown(void)
	{
		speed_val -= 100;
		if(speed_val<=0) speed_val=0;
		softPwmWrite (Motor1_PWM, speed_val);
		softPwmWrite (Motor2_PWM, speed_val);
		softPwmWrite (Motor3_PWM, speed_val);
		softPwmWrite (Motor4_PWM, speed_val);
		printf("减速: %d %%\n",speed_val);
	}

	void Cleanup(void)
	{
		this->stop();
		
		pinMode(Motor1_PWM,INPUT);
		pinMode(Motor2_PWM,INPUT);
		pinMode(Motor3_PWM,INPUT);
		pinMode(Motor4_PWM,INPUT);
		
		pinMode(Motor1_IN0,INPUT);
		pinMode(Motor1_IN1,INPUT);
		
		pinMode(Motor2_IN0,INPUT);
		pinMode(Motor2_IN1,INPUT);
		
		pinMode(Motor3_IN0,INPUT);
		pinMode(Motor3_IN1,INPUT);
		
		pinMode(Motor4_IN0,INPUT);
		pinMode(Motor4_IN1,INPUT);

	}

};
#endif
